import cv2 as cv
import numpy as np
import pyrealsense2 as rs
import socket

import matplotlib.pyplot as plt

from PIL import Image
# import zxing  # 导入解析包
import pyzbar.pyzbar as pyzbar

font = cv.FONT_HERSHEY_SIMPLEX  # 设置字体样式
data = ''
delay = 0
send = 1
bardata = 0

# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()

# Get device product line for setting a supporting resolution
pipeline_wrapper = rs.pipeline_wrapper(pipeline)
pipeline_profile = config.resolve(pipeline_wrapper)
device = pipeline_profile.get_device()
device_product_line = str(device.get_info(rs.camera_info.product_line))

found_rgb = False
for s in device.sensors:
    if s.get_info(rs.camera_info.name) == 'RGB Camera':
        found_rgb = True
        break
if not found_rgb:
    print("The demo requires Depth camera with Color sensor")
    exit(0)

if device_product_line == 'L500':
    #config.enable_stream(rs.stream.color, 960, 540, rs.format.bgr8, 30)
    config.enable_stream(rs.stream.color, 960, 540, rs.format.bgr8, 30)
else:
    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

# Start streaming
pipeline.start(config)


ip_remote = '127.0.0.1' # upboard IP
port_remote = 8887 # port
udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # setup socket

max_x = 0
max_center = 0
done = 0

def printline(str):
    print("\r", "                    ", end='')
    print("\r", str, end='')

while 1:
    # Wait for a coherent pair of frames: depth and color
    data = '-1'
    max_center = -1
    frames = pipeline.wait_for_frames()
    color_frame = frames.get_color_frame()
    if not color_frame:
        continue
    
    # Convert images to numpy arrays
    frame = np.asanyarray(color_frame.get_data()) # color_image
    #frame = cv.cvtColor(color_image, cv.COLOR_BGR2HSV) # hsvFrame
    
    frame = gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)  # 转换为灰色通道
    # hsv = cv.cvtColor(frame, cv.COLOR_BGR2HSV)  # 转换为HSV空间

    # plt.subplot(121),plt.imshow(gray,'gray')
    # plt.xticks([]),plt.yticks([])

    if delay:
        if delay%40 == 0:
            print('.', end='')
        delay = delay - 1
        continue

    circles = cv.HoughCircles(gray,cv.HOUGH_GRADIENT,1,600,param1=100,param2=55,minRadius=5,maxRadius=300)  
    if circles is not None:  # 如果识别出圆
        for circle in circles[0]:
            #  获取圆的坐标与半径
            x = int(circle[0])
            y = int(circle[1])
            r = int(circle[2])
            cv.circle(frame, (x, y), r, (0, 0, 255), 3)  # 标记圆
            cv.circle(frame, (x, y), 3, (255, 255, 0), -1)  # 标记圆心
            text = 'x:  '+str(x)+' y:  '+str(y)
            # cv.putText(frame, text, (10, 30), font, 1, (0, 255, 0), 2, cv.LINE_AA, 0)  # 显示圆心位置
            data = str(x)
            printline(x)
    else:
        # 如果识别不出，显示圆心不存在
        # cv.putText(frame, 'x: -    y: -   ', (10, 30), font, 1, (0, 255, 0), 2, cv.LINE_AA, 0)
        img = Image.fromarray(gray)
        barcodes = pyzbar.decode(img)
        # for barcode in barcodes:
        #     bardata = barcode.data.decode("utf-8")
        # im.save('tmp.jpg')
        # zx = zxing.BarCodeReader()  # 调用zxing二维码读取包
        # zxdata = zx.decode('tmp.jpg')  # 图片解码
        # print(barcodes)
        if barcodes != []:
            done = 1
            delay = 200
            qrcodedata = barcodes[0].data.decode("utf-8")
            if qrcodedata == 'get down':
                data = str(-10)
                printline('get down')
            elif qrcodedata == 'nod':
                data = str(-11)
                printline('nod')
            elif qrcodedata == 'turn left':  # 返回记录的内容
                data = str(-12)
                printline('turn left')
            elif qrcodedata == 'turn right':
                data = str(-13)
                printline('turn right')
        else:
            printline('--')
    if send:
        udp_socket.sendto(data.encode('utf-8'), (ip_remote, port_remote))
    # if not done:
    #     udp_socket.sendto(data.encode('utf-8'), (ip_remote, port_remote))

    cv.imshow('frame', frame)
    k = cv.waitKey(5) & 0xFF
    if k == 27:
        break
    elif k == 8:
        send = not send
        if not send:
            printline('not send')
    
cv.destroyAllWindows()
